#pragma once

#include "outline/type_traits.h"

namespace outline {

/// Given a point set, where each point is associated with a time, 
/// motion_line_estimator provides an owner with summed discrepancy of the 
/// intermediate points from places on the line from the first to the last points
/// with corresponding times (assuming that motion along the line is made with constant velocity).
/// Incremental modification of the point set is allowed.
template <typename T>
class motion_line_estimator;

template <typename T>
struct timed_point
{
	point<T> p;
	T t;
	timed_point() { }
	timed_point(point<T> const & pp, T tt) : p(pp), t(tt) { }
	timed_point & operator -=(timed_point const & x)
		{ p -= x.p; t -= x.t; return * this; }
	///compares coordinates only (used to test closure of outline)
	bool operator ==(timed_point const & x) const
		{ return p == x.p; }
};

typedef timed_point<unsigned short> timed_point_us;
typedef timed_point<short>	timed_point_s;
typedef timed_point<int>	timed_point_i;
typedef timed_point<float>	timed_point_f;
typedef timed_point<double>	timed_point_d;

template <typename T>
class motion_line_estimator< timed_point<T> > {
public:
	typedef timed_point<T> sample;
	struct patch {};
public:
	typedef typename type_traits<T>::accumulator_type accumulator;
	typedef point<accumulator> point_accumulator;
public:
	motion_line_estimator() { }
	motion_line_estimator(sample const & s)
		{ start(s); }
	static int min_step_len()
		{ return 2; }
public: // methods, required by simplifier
	///forgets accumulated information and sets new start point of line
	void start(sample const & s)
		{ s0 = s; ss = ts.x = ts.y = tt = 0; }
	///accumulates information about else one intermediate point
	void add_sample(sample s) {
		s -= s0;
		ss += square(s.p);
		ts += s.t * s.p;
		tt += square(s.t);
	}
	///accumulates one point and returns discrepancy,
	///optional argument may return parameters of the line (reserved for objects other from lines)
	double add_sample_and_estimate(sample s, patch * = 0, bool swap_ends = false) {
		s -= s0;
		accumulator rt = 1 / s.t;
		double d = ss - 2 * rt * scalar_product(point_accumulator(s.p), ts) + 
			square(rt * point_accumulator(s.p)) * tt;
		ss += square(s.p);
		ts += s.t * s.p;
		tt += square(s.t);
		return std::max(d, double(0)); //d may be slightly less than zero for strick lines due to rounding errors
	}
	///same as add_sample_and_estimate(...) but adding point coincides with start point
	///typically this method is called the latest for closed contours
	double add_start_and_estimate(sample const & s, patch * = 0, bool swap_ends = false) {
		assert (s == s0);
		return ss;
	}
	///wherever possible added and final points are, discrepancy will not be less than returned value
	double min_discrepancy() const
		{ return ss - square(ts) / tt; }
private:
	sample s0;
	accumulator			ss; ///< sum of intermediate points' squares
	point_accumulator	ts; ///< sum of intermediate points' times multiplied on their positions
	accumulator			tt; ///< sum of intermediate points' times
};

template <typename T>
struct timed_points
{
	std::vector< point<T> > p;
	T t;
	size_t size() const
		{ return p.size(); }
	timed_point<T> operator [] (size_t i) const
		{ return timed_point<T>(p[i], t); }
	///compares coordinates only (used to test closure of outline)
	bool operator ==(timed_points const & x) const
		{ return p == x.p; }
};

typedef timed_points<unsigned short> timed_points_us;
typedef timed_points<short>		timed_points_s;
typedef timed_points<int>		timed_points_i;
typedef timed_points<float>		timed_points_f;
typedef timed_points<double>	timed_points_d;

/// best lines for all the channels
template <typename T>
class motion_line_estimator< timed_points<T> > {
public:
	typedef timed_points<T> sample;
	struct patch {};
public:
	motion_line_estimator() { }
	motion_line_estimator(sample const & s)
		{ start(s); }
	static int min_step_len()
		{ return 2; }
public:
	void start(sample const & s) {
		channels.resize(s.size());
		for (size_t i = 0; i < channels.size(); ++i)
			channels[i].start(s[i]);
	}
	void add_sample(sample const & s) {
		assert (s.size() == channels.size());
		for (size_t i = 0; i < channels.size(); ++i)
			channels[i].add_sample(s[i]);
	}
	double add_sample_and_estimate(sample const & f, patch * = 0, bool swap_ends = false) {
		assert (f.size() == channels.size());
		double m = 0;
		for (size_t i = 0; i < channels.size(); ++i)
			m += channels[i].add_sample_and_estimate(f[i]);
		return m;
	}
	double add_start_and_estimate(sample const & f, patch * = 0, bool swap_ends = false) {
		assert (f.size() == channels.size());
		double m = 0;
		for (size_t i = 0; i < channels.size(); ++i)
			m += channels[i].add_start_and_estimate(f[i]);
		return m;
	}
	double min_discrepancy() const {
		double m = 0;
		for (size_t i = 0; i < channels.size(); ++i)
			m += channels[i].min_discrepancy();
		return m;
	}
private:
	std::vector< motion_line_estimator< timed_point<T> > > channels;
};

} //namespace outline
